﻿using System;
using System.Collections.Generic;

namespace HCore
{
    [Flags]
    public enum SteerType
    {
        Seek = 1 << 0,
        Arrive = 1 << 1,
        Separation = 1 << 2,
        Align = 1 << 3,
        Cohesion = 1 << 4,
    }

    public class Steer<T> where T : MoveEntity
    {
        private readonly T _owner;
        private SteerType _flags;
        private Vector3 _steeringForce;
        private Vector3 _target;
        private List<T> _neighbors;

        public Steer(T owner)
        {
            _owner = owner;
        }

        public Vector3 Calculate()
        {
            _steeringForce = Vector3.zero;
            CalculatePrioritized();
            return _steeringForce;
        }

        private void CalculatePrioritized()
        {
            if (On(SteerType.Seek))
            {
                if (!AccumulateForce(Seek(_target)))
                    return;
            }

            if (On(SteerType.Arrive))
            {
                if (!AccumulateForce(Arrive(_target)))
                    return;
            }

            if (On(SteerType.Separation))
            {
                if (!AccumulateForce(Separation()))
                    return;
            }

            if (On(SteerType.Align))
            {
                if (!AccumulateForce(Align()))
                    return;
            }

            if (On(SteerType.Cohesion))
            {
                if (!AccumulateForce(Cohesion()))
                    return;
            }
        }

        private bool AccumulateForce(Vector3 addForce)
        {
            Fix magnitudeSoFar = _steeringForce.magnitude;

            Fix magnitudeRemaining = _owner.maxForce - magnitudeSoFar;

            if (magnitudeRemaining <= 0)
                return false;

            Fix magnitudeToAdd = addForce.magnitude;
            if (magnitudeToAdd < magnitudeRemaining)
                _steeringForce += addForce;
            else
                _steeringForce += addForce.normalized * magnitudeRemaining;

            return true;
        }

        public void SetTarget(Vector3 target)
        {
            _target = target;
        }

        public bool On(SteerType bt)
        {
            return (_flags & bt) > 0;
        }

        public void SeekOn()
        {
            _flags |= SteerType.Seek;
        }

        public void SeekOff()
        {
            if (On(SteerType.Seek)) _flags ^= SteerType.Seek;
        }

        private Vector3 Seek(Vector3 target)
        {
            Vector3 desiredVelocity = (target - _owner.position).normalized
                                      * _owner.maxSpeed;

            return desiredVelocity - _owner.velocity;
        }

        public void ArriveOn()
        {
            _flags |= SteerType.Arrive;
        }

        public void ArriveOff()
        {
            if (On(SteerType.Arrive)) _flags ^= SteerType.Arrive;
        }

        private Vector3 Arrive(Vector3 target)
        {
            Vector3 ToTarget = target - _owner.position;

            Fix dist = ToTarget.magnitude;

            if (dist <= 0)
            {
                return Vector3.zero;
            }

            if (dist > 1)
            {
                return Seek(target);
            }

            Fix DecelerationTweaker = Fix.Ratio(3, 10);

            Fix speed = dist / (2 * DecelerationTweaker);

            speed = FixMath.Min(speed, _owner.maxSpeed);

            Vector3 desiredVelocity = ToTarget * speed / dist;

            return desiredVelocity - _owner.velocity;
        }

        public void SetNeighbors(List<T> neighbors)
        {
            _neighbors = neighbors;
        }

        public void SeparationOn()
        {
            _flags |= SteerType.Separation;
        }

        public void SeparationOff()
        {
            if (On(SteerType.Separation)) _flags ^= SteerType.Separation;
        }

        private Vector3 Separation()
        {
            Vector3 steeringForce = Vector3.zero;
            foreach (var curNeighbor in _neighbors)
            {
                if (curNeighbor == _owner || _owner.position == curNeighbor.position ||
                    (curNeighbor.position - _owner.position).sqrMagnitude > 1)
                {
                    continue;
                }

                Vector3 ToAgent = _owner.position - curNeighbor.position;

                steeringForce += ToAgent.normalized / ToAgent.magnitude;
            }

            return steeringForce;
        }

        public void AlignOn()
        {
            _flags |= SteerType.Align;
        }

        public void AlignOff()
        {
            if (On(SteerType.Align)) _flags ^= SteerType.Align;
        }

        private Vector3 Align()
        {
            Vector3 sum = Vector3.zero;
            int count = 0;
            foreach (var curNeighbor in _neighbors)
            {
                if (curNeighbor == _owner || _owner.position == curNeighbor.position ||
                    (curNeighbor.position - _owner.position).sqrMagnitude > 1)
                {
                    continue;
                }

                count++;
                sum += curNeighbor.velocity;
            }

            if (count == 0)
            {
                return sum;
            }

            return sum.normalized * _owner.maxSpeed;
        }

        public void CohesionOn()
        {
            _flags |= SteerType.Cohesion;
        }

        public void CohesionOff()
        {
            if (On(SteerType.Cohesion)) _flags ^= SteerType.Cohesion;
        }

        private Vector3 Cohesion()
        {
            Vector3 sum = Vector3.zero;
            int count = 0;
            foreach (var curNeighbor in _neighbors)
            {
                if (curNeighbor == _owner || _owner.position == curNeighbor.position ||
                    (curNeighbor.position - _owner.position).sqrMagnitude > 1)
                {
                    continue;
                }

                count++;
                sum += curNeighbor.position;
            }

            if (count == 0)
            {
                return sum;
            }

            return Seek(sum / count);
        }
    }
}